#pragma once
#include "CAction.h"
//#include "CQueryTimer.h"

#define POSITION_CONTROL_ID 13
#define POS_MAX_FORCE 0.06
#define DEFAULT_KP_POS 0.1
#define DEFAULT_KD_POS 0.02
#define DEFAULT_KI_POS 0.1

using namespace std;

class CPositionControl :
	public CAction
{
public:
	CPositionControl(void);
	CPositionControl( CSeaAmp &seaAmp ) { Init( seaAmp ); }
	~CPositionControl(void);

	bool Init( CSeaAmp &seaAmp ) { return CAction::Init( seaAmp, POSITION_CONTROL_ID ); }

	void DoAct( void );

	void SetKP( double p ) { Kp = p; }
	double GetKP( void ) { return Kp; }

	void SetKI( double i ) { Ki = i; }
	double GetKI( void ) { return Ki; }

	void SetKD( double d ) { Kd = d; }
	double GetKD( void ) { return Kd; }
	
private:
	double Kp;
	double Ki;
	double Kd;

	double delta[3];

	double output;

	ofstream PositionData;

	//CQueryTimer timer;
	double timeInterval;
};

